function [zz_final,Nef1, flagobs,NfSitu,printFlag,smoothFin,failFlag,sucFlag]=...
    smooth_old(smooth_flag,smooth_pre,z0center,i,path0,nefpath,obs00,flagplot,...
    ParameterMax0,safedistance, zz_in, nef_in,lamobj,...
    dmax,flaglift,etadw,lamc2,vdmax,vwmax,NfSitu_in,flagrot, jj, nefjj, signvpath,mateWallVector,fail_in,suc_in,task)
ParameterMax=[reshape(ParameterMax0(1:5),1,5),vdmax(1),vwmax(1)];
L=1.7184;c0max=tan(dmax/2)/L;
wmax=min(0.32,ParameterMax(5));c1max=wmax/L/2;c2max=c1max/6/etadw;
c2maxref=0.012/etadw;
cmax=[c0max,c1max,c2max];
printFlag = 0;
index=jj(i,:);
path=path0;
%% 通过i取当前段的有效路径index
if (i>1)&&(i<nefjj)
    flaglift=0;
end
if i>1
    flagrot=1;
end
%% 添加原地转向限制逻辑，非起始段不做限制，起始段根据flaglift做不同限制
if i<nefjj%非到达终点，可以放松对终点的约束
    epf=[0.5,0.1,0.1]/21;
else
    epf=[0.05,0.01,0.01];
end
%% 添加修改打印输入
smoothFin =1;
Ndata=10000;%4e3;
%% 根据修改的输入接口更改lis定义方式
zz_final=zeros(Ndata,7);
signvj=signvpath(i);
if signvj>0
    zpre=ctof([z0center(1),z0center(2),z0center(3),z0center(4)]);
else
    zpre=ctor([z0center(1),z0center(2),z0center(3),z0center(4)]);
end
if i < nefjj
    zf = cToC(path,index,signvj);
else
    zf = [path(index(2),:),0];
end
[flagend]=checkobs_(obs00,zf,1*max(0.25,safedistance*0.5),flaglift<0,3.4);
%% 添加平滑前障碍检查，并根据是否冲突进行参数调整
if flagend<=0
    epf=[0.8,0.02,0.01];% epy0=0.93/4;epy1=0.21/2;epy2=0.95;
end
idpath=jj(i,1):jj(i,2);
pathi = zeros(2000,3);validIndPathi = length(idpath);
pathi(1:validIndPathi,:) = path(idpath,:);
[pathi_,nefpathi]=fexpand_(pathi,validIndPathi,0*z0center(4)); %有转向角会造成问题
[obs0]=simplifyobs(obs00,pathi_,nefpathi);%根据路径规划轨迹简化障碍物
if signvj>0
    pathref=pathi_;nefpathref=nefpathi;
    zff=ctof(zf);
    [zz_head,nf_situ,Nef]=plan(zpre,zff,pathref, ...
        nefpathref,obs0,ParameterMax,flagplot,safedistance*1.1,lamobj,cmax,epf, ...
        c2maxref,lamc2,flagrot,0,mateWallVector,signvj);
else
    zprer=zpre;zprer(3)=zpre(3)+pi;zprer(4)=-zpre(4);
    zfr=ctor(zf);zfr(3)=zfr(3)+pi;%zfr(4)=-zfr(4);
    nefpathref=nefpathi;pathref=pathi_;pathref(1:nefpathref,3)=pathi_(1:nefpathref,3)+pi;
    [zz_head,nf_situ,Nef]=plan(zprer,zfr,pathref,nefpathref,obs0, ...
        ParameterMax,flagplot,safedistance*1.1,lamobj,cmax,epf,c2maxref,lamc2, ...
        flagrot,0,mateWallVector,signvj);
    zz_head(:,3)=zz_head(:,3)-pi;
    zz_head(:,[4,5,6,7])=-zz_head(:,[4,5,6,7]);
end
%% 对规划后的轨迹进行障碍检查
zz_head(1:Nef,3)=fround3(zz_head(1:Nef,3));
zz_final(1:Nef,:)=zz_head(1:Nef,:);
if eta > 1
    NfSitu = floor(eta * nf_situ);
else
    NfSitu = nf_situ;
end
Nef1 =cutend(zz_final,Nef,0.02);
end
function [out,gout,H,f,ger,ber,gc0,bc0,gc1,bc1,gc2,bc2,gc3,bc3,udsol,gud,bud]=HfAb0(cref,y1ref,u,A1,b1,A2,b2,~,~,A0d,b0d,A1d,b1d,A2d,b2d,A3d,b3d,Dy0d,flagw)
n=numel(u);nd=size(A0d,2);
y1=A1*u+b1;
y2=A2*u+b2;
yq=(y1.^2+1);
y1y2=y1.*y2;
curv=y2./yq.^(3/2)+cref./sqrt(yq);%前进 %后退
gcurv=zeros(size(A1,1),n);
gcurvy2=(1./yq.^(3/2));
gcurvy1=(-(3*y1y2)./yq.^(5/2))-y1.*cref./yq.^(3/2);
for j=1:n
    gcurv(:,j)=gcurvy1.*A1(:,j)+gcurvy2.*A2(:,j);
end
dss=(1./sqrt(1+y1ref.^2));
ds2=dss.^2;ds3=dss.*ds2;
for j=1:size(A1d,2)
    A1d(:,j)=dss.*A1d(:,j);
    A2d(:,j)=(ds2).*A2d(:,j);
    A3d(:,j)=(ds3).*A3d(:,j);
end
b1d=dss.*b1d;
L=1.7184;
Ad=A0d+flagw*L*A1d;bd=b0d+flagw*L*b1d;lambda=0.00000025;%后退：c0+c1*(-cos(d))*L 整体 c0+c1*(-L*cos(delta)*(vref<0)+L*vref>0)) cos(delta)=(1-c^2)/(1+c^2)
invAd=(Ad'*Ad+lambda*eye(nd))\(Ad');%Ad*ud+bd-curv
udsol=invAd*(curv-bd*Dy0d);%(invAd-invAd*bd*li)*curv
cons0=A0d*udsol+b0d*Dy0d;
cons1=A1d*udsol+b1d*Dy0d;%
cons2=A2d*udsol+b2d*Dy0d;
cons3=A3d*udsol+b3d*Dy0d;
er=Ad*udsol+bd*Dy0d-curv;
invAd1gcurv=invAd*gcurv;
gc0=A0d*invAd1gcurv;
gc1=A1d*invAd1gcurv;
gc2=A2d*invAd1gcurv;
gc3=A3d*invAd1gcurv;
ger=Ad*invAd1gcurv-gcurv;
gud=invAd1gcurv;
bc1=cons1-gc1*u;
bc0=cons0-gc0*u;
bc2=cons2-gc2*u;
bc3=cons3-gc3*u;
ber=er-ger*u;
bud=udsol-gud*u;
H=0;f=0;out=0;gout=0;
end
function zz = cToC(path,index,signv)
lf=1.7184;
segEnd = path(index(end),:);
diffXY = diff(path(index(1):index(end),1:2));
pa = path(index(1):index(end),:);
ds = sqrt(diffXY(:,1).^2+diffXY(:,2).^2);
cumDs = cumsum(flip(ds));
n=numel(cumDs);
lengthcurv=max(1,min(n,sum(cumDs<lf)+1));
segBegin=pa(n-lengthcurv+1,:);
ep = 1e-5;
dL =signv*cumDs(lengthcurv);
dTh = (segEnd(3)-segBegin(3));
dTh = dTh+(dTh>0)*ep+(dTh<=0)*(-ep);
dTh = mod(dTh,2*pi);
dTh(dTh>pi) = dTh(dTh>pi)-2*pi;
R = dL/dTh;
delta = atan(lf/R)*2;
perpendicularL = lf*sin((delta/2));
x = segEnd(1)+perpendicularL*sin(segEnd(3));y=segEnd(2)-perpendicularL*cos(segEnd(3));
zz = [x,y,segEnd(3)+delta/2,delta];
end
function [S,dss,ss]=calss(zz)
dxx=diff(zz(:,1));
dyy=diff(zz(:,2));
dss=sqrt(dxx.^2+dyy.^2);
S=sum(abs(dss));
if nargout>2
    ss=cumsum(dss);
end
end
function [th]=calth(zz,signv)
n=size(zz,1);
dz=diff(zz(:,1:2));
th=zeros(n,1);
for i=1:n-1
    th(i)=gett3(dz(i,:),signv);
end
th(n)=th(n-1);
end
function [zzr,yref,jref]=calyfrenet_simple(zz,zzref,flagplot)
n=size(zz,1);
yref=zeros(n,1);
id=zeros(n,1);
cth=cos(zzref(:,3));
sth=sin(zzref(:,3));
for i=1:size(zz,1)
    zi=zz(i,:);
    er=[zi(1)-zzref(:,1),zi(2)-zzref(:,2)];
    %选择目标轨迹上的对应点：1.保证单调性;2. 保证不能离上一个参考点对应的目标点距离太远
    normer=sqrt(sum(er.^2,2));
    [~,j]=min(normer);
    yref(i)=er(j,1)*(-sth(j))+er(j,2)*cth(j);%der(j);
    id(i)=j;
end
zzr=[zzref(id,1)+yref.*(-sin(zzref(id,3))),zzref(id,2)+yref.*cos(zzref(id,3))];
jref=id;
end
function zf=ctof(z)
L=1.7184;
th=z(:,3);
zf=z;
zf(:,1:3)=[z(:,1)+L*cos(th),z(:,2)+L*sin(th),th];
end
function zr=ctor(z)
L=1.7184;
th=z(:,3);
zr=z;
if size(z,2)>3
    d=z(:,4);
else
    d=0*z(:,1);
end
zr(:,1:3)=[z(:,1)-L*cos(th-d),z(:,2)-L*sin(th-d),th-d];
end
function nef1 =cutend(zz,nef,dt)
n1=max(1,min(nef,1e4));v=zz(1:n1,5);
w=zz(1:n1,6);
li=cumsum(abs(v)*dt);
lid=cumsum(abs(w)*dt);thtoend=lid(end)-li;
stoend=li(end)-li;id=(thtoend<0.1)&(stoend<0.1)&(abs(v)<0.05)&(abs(w)<0.005);%(vtoend<1);%&(dtoend<0.1);
nef1=nef;
for j=1:numel(id)
    if id(j)==1
        nef1=min(nef,j); break;
    end
end
end
function [er,zzout,ssout,dssout]=fit2(z0,zf,curv0,curvf,sref0,zzref0,cref0,vref0,c0max,c1max,~,n,K,flagplot,obs,ParameterMax,rsafe,ep00,lamobj,epf,lamc20,flaglift,signCar)
z=z0;%zz(1,:);
nd=n*1;Kd=n*K/nd;md=2;m=3;
maxiter=2000;
%zzref0,cref0,参考线
%sref0,参考线距离网格
%% 对齐参考线和目标线的终点
th=zzref0(:,3);
dth=diff(th);
dth=mod(dth,2*pi);
dth(dth>pi)=dth(dth>pi)-2*pi;
th=cumsum([th(1);dth]);
zzref0(:,3)=th;
[~,y0ref,jstart]=calyfrenet_simple(z0,zzref0);%计算zz在参考轨迹zzref下的纵坐标
[~,~,jend]=calyfrenet_simple(zf,zzref0,0);%计算zz在参考轨迹zzref下的纵坐标
Smax=sref0(jend)-sref0(jstart);
%用平滑后的线作为参考线
%% 计算参考线的网格点，曲率，z
dsref=(Smax-0.0000001)/n/K;
sref=dsref*(1:n*K)';
cref=interp1(sref0,cref0,sref+sref0(jstart));
vref=interp1(sref0,vref0,sref+sref0(jstart));
vref(vref>0)=1;vref(vref<=0)=-1;
[zzref]=model(zzref0(jstart,:),cref,dsref,vref);%用模型生成参考线---这里zzref对zzref0容易出现误差,因为插值容易造成cref误差，而cref误差又容易造成积分误差
if norm(zzref(end,1:2)-zf(1:2))>0.2
    zzref=interp1(sref0,zzref0,sref+sref0(jstart));
end
[~,yfref,~]=calyfrenet_simple(zf,zzref,0);%重新校正误差
dcurv0=0;
Dc0=[curv0;dcurv0;0;0;0;0;0];Dc0=Dc0(1:md);
dy0=y0ref;
signv=sign(mean(sign(vref)));
dy2=(curv0-cref(1)/(1+dy1^2)^(1/2))*(1+dy1^2)^(3/2);
Dy00=[dy0;dy1;dy2;0;0;0;0];Dy0=1*Dy00(1:m);
[~,~,A0,b0,A1,b1,A2,b2,A3,b3]=polyfun(n,Smax,Dy0,m,K);
[~,~,A0d,B0d,A1d,B1d,A2d,B2d,A3d,B3d]=polyfunB(nd,Smax,md,Kd);
yref=zeros(n*K,1);y1ref=zeros(n*K,1);
y0f=yfref;
y1f=tan(signv*(zf(3)-zzref(end,3)));y2f=(curvf-cref(end)/(1+y1f^2)^(1/2))*(1+y1f^2)^(3/2);
maxkss=8;
copt=zeros(n*K,1);
omega=zeros(n*K,1);
delta=zeros(n*K,1);
lamobscon=1;
for ksss=1:2
    u=zeros(n,1);
    ep0=ep00;
    epy0=epf(1);epy1=epf(2);epy2=epf(3);
    eper=0.5;1.000801;
    c2max=c1max/0.5;c2maxk=c2max/1;
    lamobs=20;lamer=1;
    lamc0=200;lamc1=10;
    lamc2=lamc20;2000;
    lamy0f=1000;lamy1f=500;lamy2f=0.1;    %lamy0f=10;lamy1f=50;lamy2f=0.1;
    c0maxk=c0max*1.0;
    vmax=ParameterMax(1);
    vmaxk=1.5;
    rsafek=rsafe;%min(2,rsafe+0.5);
    yobmin=-5*ones(n*K,1);yobmax=5*ones(n*K,1);
    ep0k=ep0;
    %人工添加的障碍点
    y0max=5;
    i10=[1,K:(1*K):n*K];
    zobi=[zzref(i10,1)-(1.7+y0max)*sin(zzref(i10,3)),zzref(i10,2)+(1.7+y0max)*cos(zzref(i10,3)),ones(numel(i10),1);
        zzref(i10,1)+(1.7+y0max)*sin(zzref(i10,3)),zzref(i10,2)-(1.7+y0max)*cos(zzref(i10,3)),-ones(numel(i10),1)];
    for kss=1:maxkss
        L=1.7184;
        flagw=1;
        [~,~,~,~,ger,ber,gc0,bc0,gc1,bc1,gc2,bc2,gc3,bc3,~,~,~]=...
            HfAb0(cref,y1ref,u,A1,b1,A2,b2,A3,b3,A0d,B0d,A1d,B1d,A2d,B2d,A3d,B3d,Dc0,flagw);
        a0f=A0(end,:);b0f=b0(end);
        a1f=A1(end,:);b1f=b1(end);
        a2f=A2(end,:);b2f=b2(end);
        lam0=0;
        lam1=lamobj(1);lam2=lamobj(2);lam3=.001;
        lamc1obs=0;lamc2obs=3000;lamc3obs=0;
        H=1*(a0f')*a0f+1*(a1f')*a1f+lam0*(A0'*A0)+lam1*(A1'*A1)+lam2*(A2'*A2)+lam3*(A3'*A3)+lamc1obs*(gc1'*gc1)+lamc2obs*(gc2'*gc2)+lamc3obs*(gc3'*gc3);
        f=1*a0f'*(b0f-y0f)+1*a1f'*(b1f-y1f)+lam0*A0'*(b0)+lam1*A1'*(b1)+lam2*A2'*b2+lam3*(A3'*b3)+lamc1obs*(gc1'*bc1)+lamc2obs*(gc2'*bc2)+lamc3obs*(gc3'*bc3);
        id1=[1,K:(1*K):n*K]';
        [Aobs_,bminobs,bmaxobs,~,dref] = obs_des(A0(id1,:),b0(id1,:),A1(id1,:),b1(id1,:),zzref(id1,:),[obs(:,1:2);zobi(:,1:2)],u,rsafek+1.5,signCar);
        Aobs_=double(Aobs_);bminobs=double(bminobs);bmaxobs=double(bmaxobs);
        scale_obs=(0.0001+1*(kss>0)*lamobscon)*ones(size(bminobs));
        idy1=1:K:n*K;   y1max=tan(pi/6); scale_y1=1000*ones(numel(idy1),1);%该约束需要加强才能保证
        epc0=0.1;
        vmaxkk=ones(size(sref))*2;
        c1maxk=c0max/2.5;
        c1maxkk=c1maxk./vmaxkk;%lamc1=1e1;
        lamc0f=20;
        epc0f=0.01;
        A0_=A0;b0min=-b0+max(yobmin,yref-ep0k);b0max=-b0+min(yobmax,yref+ep0k);
        scaley0=lamobs*ones(size(b0));
        As=[A0_;ger;gc0;gc0(1,:);gc0(end,:);gc1(end,:);gc1;gc2;a0f;a1f;a2f;A1(idy1,:);Aobs_];%c0max=0.3;
        bmin=[b0min;-ber-eper;-bc0-c0maxk;-bc0(1)+Dc0(1)-epc0;-bc0(end)+curvf-epc0f;-bc1(end)-0.21/1.7;-bc1-c1maxkk;-bc2-1*c2maxk;-b0f+y0f-epy0;-b1f+y1f-epy1;-b2f+y2f-epy2;-b1(idy1,:)-y1max;bminobs];
        bmax=[b0max;-ber+eper;-bc0+c0maxk;-bc0(1)+Dc0(1)+epc0;-bc0(end)+curvf+epc0f;-bc1(end)+0.21/1.7;-bc1+c1maxkk;-bc2+1*c2maxk;-b0f+y0f+epy0;-b1f+y1f+epy1;-b2f+y2f+epy2;-b1(idy1,:)+y1max;bmaxobs];
        scaleb=[scaley0;lamer*ones(size(ber));lamc0*ones(size(bc0));lamc0f;0.01;0.01;lamc1*ones(size(bc1));lamc2*ones(size(bc2));lamy0f;lamy1f;lamy2f;scale_y1;scale_obs];
        LB=-1.5*ones(n,1);UB=-LB;
        scalex=ones(n,1);
        [u]=myquadprog(u,H,f,As,bmin,bmax,LB,UB,maxiter,scaleb,scalex,1.6,0.25*1,0.510,1e-3);
        y0=A0*u+b0;y1=A1*u+b1;y2=A2*u+b2;%y3=A3*u+b3;
        yq=(y1.^2+1);
        erob=min([y0-yobmin;yobmax-y0]);
        erc0=min(c0maxk-abs(gc0*u+bc0));
        if (erc0<-c0maxk*0.025)||(erob<-0.05)||min(dref)<1.7184+0.2
            epy0=min(1,2*epy0);lamy0f=lamy0f/1;
            epy1=min(0.2,4*epy1);lamy1f=lamy1f/1;
            epy2=min(0.1,4*epy2);
            vmaxk=max(1,min(3,vmaxk/1.5));
            lamc1=lamc1/3;
            lamc2=lamc2/4;
            c2maxk=min(0.5,c2maxk*4);
        end
        if erob<0
            if kss>1
                lamobj=max([5,1],lamobj/4);
            end
            
        end
        if erc0<0
            lamc0=min(2500,lamc0*4);
             lamobj=max([0.5,0.1],lamobj/2);
        end
        if (erc0>=c0maxk/10)&&(erob>0.5)
            vmaxk=max(0.2,min(vmax,vmaxk*1.5));
            c2maxk=max(0.0025,c2maxk/1.5);
        end
        if (erob>1)||(erc0>c0maxk/6)
            rsafek=max(rsafe,rsafek*0.6);
        end
        if erob<0
        end
        if min(dref)<1.7184+0.1
            lamobscon=min(1000,lamobscon*4);
            if kss>3
            end
            lamobj=max([0.5,0.1],lamobj/1);
        elseif erc0<0
            lamobscon=max(0.1,lamobscon/3);
        end
        flagobs = 0;
        zz21 = zeros(size(zzref,1),1);
        th21 = zeros(size(zzref,1),1);
        if (kss>212323)||(kss==maxkss)
            [~,~,~,~,~,~,~,~,~,~,~,~,~,~,udsol,~,~]=...
                HfAb0(cref,y1ref,u,A1,b1,A2,b2,A3,b3,A0d,B0d,A1d,B1d,A2d,B2d,A3d,B3d,Dc0,flagw);
            cy=y2./yq.^(3/2);
            copt=(1.*cy+1.*cref./sqrt(yq));
            dss=1./sqrt(1+y1.^2);
            cd0=A0d*udsol+B0d*Dc0;
            cd1=diag(dss)*(A1d*udsol+B1d*Dc0);
            omega=2*cd1*L./(1+L^2*cd0.^2);%=diff(d).*dss(2:end)/(Xf/n/K)
            cosd=(1-L^2*cd0.^2)./(1+L*L*cd0.*cd0);
            delta=sign(cd0).*acos(cosd);
            %figure;plot(diff(delta)./dss(2:end)/(dsref));hold on;plot(omega)%检查角速度计算是否正确
            %figure;d=delta;plot(sin(d)./(cos(d)+1)/L+1*omega./(cos(d)+1));hold on;plot(copt)%检查曲率和转向角的一致性
            %根据y多项式计算，这种算法不保证曲率和x,y,th的一致性，但是准确跟踪目标轨迹
            thref=calth(zzref,1);
            zz21=[zzref(:,1)+y0.*(-sin(thref)),zzref(:,2)+y0.*(cos(thref))];%由于thref(2)和thref(1)之间有差异，导致起点航向角不等于zzref(1,3)+atan(dy1)
            [c210]=getc1(zz21);c2100=c210*vref(1);c21=[c2100(1);c2100;c2100(end)];
            th210=calth(zz21,sign(mean(sign(vref))));dth210=froundth(diff(th210));th21=th210(1)+cumsum([0;dth210]);
            z(3)=th210(1);
            zz21s=[zz21,th21,delta];
               carsize=[2,2,3.4];
               [flagobs,~]=checkobscenter(obs,frtocenter(zz21s,1),1*max(0.1,rsafe*0.5),flaglift,size(zz21s,1),carsize);%max(0.5,safedistance));

        end
    end
    if flagobs>0
        break;
    else
        lamobj=[50,10]*.01;
        epf=max(epf,[0.1,0.1,0.1]);
        lamobscon=100;
    end
end
[~,dss]=calss(zz21);
%% 根据速度方向生成新的轨迹
%根据曲率来计算，保证曲率和x,y,th一致性，但是和目标轨迹有差别
[zz2]=model(z,copt*1.00,Smax/n/K*sqrt(1+y1.^2),vref); er=[norm(zz2(end,1:2)-zf(1:2)),norm(zz21(end,1:2)-zf(1:2))];
ds0=norm(zz21(1,1:2)-z(1:2));
zzout=[zz21,th21,delta,vref,omega,0*vref];
dssout=[0;ds0;dss];ssout=cumsum(dssout);
end
function th21=fround3(th210)
dth210=froundth(diff(th210));
th21=mod2pi(th210(1))+cumsum([0;dth210]);
end
function [dth1]=froundth(dth)
n=size(dth,1);
dths=[dth,dth-2*pi,dth+2*pi,dth-4*pi,dth+4*pi];
dth1=zeros(n,1);
for i=1:n
    [~,j]=min(abs(dths(i,:)));
    dth1(i)= dths(i,j);
end
end
function [A,B]=getab(t,m)
t=t(:);
if m<=4
    A=[[1, t, t^2/2, t^3/6]
        [0, 1,     t, t^2/2]
        [0, 0,     1,     t]
        [0, 0,     0,     1]];
    B=[t^4/24
        t^3/6
        t^2/2
        t];
else
    A=[[1, t, t^2/2, t^3/6, t^4/24,t^5/120,t^6/120/6]
        [0, 1,     t, t^2/2,  t^3/6,t^4/24,t^5/120]
        [0, 0,     1,     t,  t^2/2,t^3/6,t^4/24]
        [0, 0,     0,     1,      t,t^2/2,t^3/6]
        [0, 0,     0,     0,      1,t,t^2/2]
        [0, 0,     0,     0,      0,1,t]
        [0, 0,     0,     0,      0,0,1]];
    B=[t^7/720/7
        t^6/120/6
        t^5/120
        t^4/24
        t^3/6
        t^2/2
        t];
end
A=A(1:m,1:m);B=B(end-m+1:end);
end
function [curv,dth]=getc1(zz)
n=size(zz,1);
P1=zz(1:n-2,1:2);
P2=zz(2:n-1,1:2);
P3=zz(3:n,1:2);
l13=sum((P1-P3).^2,2);
l23=sum((P2-P3).^2,2);
l12=sum((P1-P2).^2,2);
s=(sqrt(l23)+sqrt(l12))/2;
cosdth=(l23+l12-l13)/2./sqrt(l23.*l12);
flag=ones(n-2,1);%速度方向
for i=1:numel(cosdth)-1
    if (i>1)&&(cosdth(i)>0)
        flag(i)=0;%V形转折点
        flag(i+1:end)=-flag(i-1);%速度方向改变
    end
end
flag(end)=flag(end-1);
%计算曲率方向，用叉积来算，叉积等于面积大小加方向，前进方向左边为正，右边为负
er13=P3-P1;er12=P2-P1;S=er12(:,1).*er13(:,2)-er12(:,2).*er13(:,1);%
%更新曲率方向，需要乘以速度方向
dth=2*(S./sqrt(l13.*l12));dth(cosdth>0)=0;
curv=dth./s.*flag;%absdth(cosdth>0)=0;
n1=numel(cosdth);
for i=1:numel(cosdth)%处理V形顶点
    if cosdth(i)>0
        if (i+1<=n1)&&(i-1>=1)
            curv(i)=(curv(i+1)+curv(i-1))/2;
        elseif i+1==n1
            curv(i)=curv(i-1);
        elseif i-1==0
            curv(i)=curv(i+1);
        end
    end
end
end
function th=gett3(dz,signv)
x=dz(1);
y=dz(2);
s=sqrt(x^2+y^2);
if s==0
    th=0;
else
    if y>=0
        th=acos(x/s);
    else
        th=-acos(x/s);
    end
end
if signv<0
    th=th+pi;
end
end
function [yymin,yymax,xx]=getyobsexpand(obs,R,signobs,Xf,n,ymax,ymin)
xx=Xf/n*(1:n)';
yymax=ymax*ones(size(xx));
yymin=ymin*ones(size(xx));
if 1
    for i=1:size(obs,1)
        x0=obs(i,1);
        y0=obs(i,2);
        id=(xx<x0+R(1))&(xx>x0-R(1));%
        if signobs(i)>0
            yymax(id)=min(yymax(id),y0-signobs(i)*R(end));
        elseif signobs(i)<0
            yymin(id)=max(yymin(id),y0-signobs(i)*R(end));
        end
    end
end
end
function [zz_new,ttnew,N1]=hecheng(DDy,zz_path,ss_path,tt,N)
ttnew=0.02*(0:N-1);%zeros(N,1);
zz_new=zeros(N,7);
ssnew=zeros(N,1);
%% 横纵向合成 x y th delta v w a dww
N0=floor(tt(end)/0.02)+1;
if N0>=N
    N1=1;
    return
end
ss=DDy(1,:)';vv=DDy(2,:)';aa=DDy(3,:)';Smax=max(ss_path);
tf=max(tt);%max(tt(ss<=Smax+0.001));
N1=sum(ttnew(1:N0)<=tf);
ssnew(1:N1)=interp1(tt,ss,ttnew(1:N1));
ssnew(ssnew>=Smax)=Smax;ssnew(ssnew<=min(ss_path))=min(ss_path);
zz_new(1:N1,[1:4,6])=interp1(ss_path,zz_path(:,[1:4,6]),ssnew(1:N1));
zz_new(1:N1,5)=interp1(tt,vv,ttnew(1:N1));
zz_new(1:N1,7)=interp1(tt,aa,ttnew(1:N1));%sin/(L*cos+L)  w*cos/(L*cos+L)-sin/(L*c+L)^2*(-s)
zz_new(1:N1,6)=zz_new(1:N1,5).*zz_new(1:N1,6);
end
function v=mod2pi(x)
v=rem(x,2*pi);
i1=(v<-pi);i2=(v>pi);
v(i1)=v(i1)+2*pi;
v(i2)=v(i2)-2*pi;
end
function [zz]=model(z,cc1,dt,vv)
if nargin<4
    vv=ones(size(cc1,1),1);
end
N=size(cc1,1);
if numel(dt)==1
    dtt=dt*ones(N,1);
else
    dtt=dt;
end
zz=zeros(N,3);
m=10;
for i=1:N
    dti=dtt(i);
    ci=cc1(i);
    vi=vv(i);
    for j=1:m
        th=z(3);
        dz=[cos(th),sin(th),ci];
        z=z+dz*dti/m*vi;
    end
    zz(i,:)=z;
end
end
function [x]=myosqp(x0,P,q,A,bmin,bmax,maxk,alpha,rho,sigma,ep)
[~,n]=size(A);
x=x0;z=A*x;y=0*z;
P1=P+sigma*eye(n)+rho*(A'*A);
invP1=inv(P1);
er10=1e5;er20=1e5;
count=0;%at=A';
for k=1:maxk
    b1=sigma*x-q+A'*(rho*z-y);
    x1=invP1*b1;
    z1=A*x1;
    x=alpha*x1+(1-alpha)*x;%x1=invP1*(sigma*x-q+A'*(rho*z-y))=sigma*invP1*x+invPA*(rho*z-y)
    znew=min(bmax,max(bmin,alpha*z1+(1-alpha)*z+1/rho*y));
    y=y+rho*(alpha*z1+(1-alpha)*z-znew);
    z=znew;
    if mod(k,10)==1
        li=A*x;
        rprim=(li-z);ep1=(ep)*max(abs(li)+abs(z)+0.0001);
        li1=P*x;li3=A'*y;rdual=li1+q+li3;ep2=(ep)*max(abs(li1)+abs(li3)+abs(q)+0.0001);
        er1=max(abs(rprim));
        er2=max(abs(rdual));
        if (er1<=ep1)&&(er2<ep2)
            %fprintf('myquadprog1 k=%i maxk=%i rprim=%f rdual=%f\n',int32(k),int32(maxk),er1/ep1*ep,er2/ep2*ep)
            break;
        end
        if (er1>=er10*0.99)&&(er2>=er20*0.99)
            count=count+1;
        else
            count=0;
        end
        er10=er1;er20=er2;
        if count>2
            if k>400
                1;
            end
            break;
        end
    end
    if k==maxk
    end
end
end
function [x]=myquadprog(x0,H,f,A,bmin,bmax,xmin,xmax,maxk,scaleb0,scalex0,alpha,rho,sigma,ep)
n=numel(xmin);
erc=0.01;
if nargin<9
    scaleb=1./max(erc,bmax-bmin);
    scalex=1./max(erc,xmax-xmin);
else
    scaleb=scaleb0./max(erc,bmax-bmin);
    scalex=scalex0./max(erc,xmax-xmin);
end
for i=1:n
    A(:,i)=A(:,i).*scaleb;
end
x=myosqp(x0,H,f,[A;diag(scalex)],[bmin.*scaleb;xmin.*scalex],[bmax.*scaleb;xmax.*scalex],maxk,alpha,rho,sigma,ep);
end

function [Aobs,bmin,bmax,minJL,dref] = obs_des(A0,b0,A1,b1,zRef,obs,u,rsafe,signv)

v0 = A0*u+b0;v1 = A1*u+b1;
xR = zRef(:,1);yR = zRef(:,2);thetaR = zRef(:,3);
g = zeros(size(zRef,1),1);
minJL = zeros(size(zRef,1),1);
% 计算当前轨迹点在frenet坐标系上的映射点
z = [xR-v0.*sin(thetaR),yR+v0.*cos(thetaR),thetaR+atan(v1)];
nref=size(zRef,1);
flag=zeros(nref,1);
if signv >= 0
    lf=5.2-1.718;
else
    lf = 4-1.718;
end
dref=10*ones(nref,1);
for i = 1:size(zRef,1)
    zi = z(i,:);
    [deltaS,deltaH] = fxy(obs,zi);
    deltaH(abs(deltaS)>lf) = 100;
    [dmin,minJ] = min(abs(deltaH));
    minJL(i) = minJ;
    if dmin>rsafe;flag(i)=1;end
    dref(i)=dmin;
    % % 直接差分法求+上eps后的极限值
end
[~,deltaHRef] = fxy(obs(minJL,:),zRef);
[~,fyy] = fxy(obs(minJL,:),z);
g = sign(deltaHRef).*fyy;
sinth = sin(thetaR);costh = cos(thetaR);
dV11 = (-sinth.*sin(thetaR+atan(v1))-costh.*cos(thetaR+atan(v1))).*sign(deltaHRef);
deltaX = obs(minJL,1)-xR+v0.*sin(thetaR);
deltaY = obs(minJL,2)-yR-v0.*cos(thetaR);
dV22 = (-deltaX.*(sinth.*-sin(atan(v1))./(1+v1.^2) + costh.*cos(atan(v1))./(1+v1.^2))+ ...
    deltaY.*(costh.*-sin(atan(v1))./(1+v1.^2)-sinth.*cos(atan(v1))./(1+v1.^2))).*sign(deltaHRef);
Aobs = dV11.*A0+ 1*dV22.*A1;bObs = g-Aobs*u;
bmin = -bObs+rsafe;
bmax = -bObs+rsafe+20;
end
function [deltaS,deltaH] = fxy(P,z)
    deltaX = P(:,1)-z(:,1);
    deltaY = P(:,2)-z(:,2);
    deltaS = deltaX.*cos(z(:,3))+deltaY.*sin(z(:,3));
    deltaH = -1*deltaX.*sin(z(:,3))+deltaY.*cos(z(:,3));
end
function [zz2,nfsitu,Nef]=plan(zpre,zf0,path,nefpath,obs,...
    ParameterMax,flagplot,safedistance,lamobj,cmax,epf,~,lamc2,flagrot,flaglift,mateWallVector,signv)
if flagrot>0
    dmax=2*flagrot*37/180*pi;
else
    dmax = .01*37/180*pi;
end
eta=fetaopt32([ctol(ftoc(zpre))],ftoc(zpre),path,nefpath,dmax,obs,safedistance,0*flagplot,mateWallVector,signv);%获取当前原地转向的终止转向角
dtarget=min(0.64,max(-0.64,zpre(4)+eta*dmax));
obsPort = [];
obs = [obs;obsPort];
[z1,zz_rot]=rot(zpre,dtarget);%计算原地转向轨迹
nfsitu=size(zz_rot,1);nefrot=nfsitu;
df=zf0(end);
maxdistoref=3;
rsafe=safedistance;
delta0=z1(4);deltaf=df;
%% 路径规划
d=delta0;curv0=sin(d)/(cos(d)+1)/1.7184;
d=deltaf;curvf=sin(d)/(cos(d)+1)/1.7184;
%% 删除get2 降低函数嵌套层次
c0max=cmax(1);c1max=cmax(2);c2max=cmax(3);
m=20;n=130;
[zzref00,sref00]=getref_bezier(path(1:nefpath,1:2),m,n);%使用贝塞尔平滑路径曲线
zzref0=zzref00(:,1:3);cref0=zzref00(:,4);sref0=sref00;N=size(zzref0,1);vref0=ones(N,1);
if flagplot
    figure(13);plot(zzref0(:,1),zzref0(:,2),'b--');
end
%需要利用参考线计算出障碍的防线
n=80;
% n=100;
K=4;
[~,zzp,ssp]=fit2(z1(1:3),zf0(1:3),curv0,curvf,sref0,zzref0,cref0,vref0,1*c0max, ...
    1*c1max,1*c2max,n,K,0,obs,ParameterMax,rsafe*1.2,maxdistoref,lamobj,epf,lamc2,flaglift,signv);
%% 速度规划
[zzj,Nj]=planv(zzp,ssp,ParameterMax,0);
% z0j=zzp(end,:);
N_final=Nj;
%% 综合可行性
%zz2=[zz_rot;zz_final(1:N_final,:)];
zz2=zeros(5000,7);
zz2(1:nefrot,:)=zz_rot(1:nefrot,:);
zz2(nefrot+1:min(5000,nefrot+N_final),:)=zzj(1:min(5000-nefrot,N_final),:);
Nef=min(size(zz2,1),N_final+nefrot);
end
function [zz_final,N_final]=planv(zz_path,ssp,ParameterMax,~)
nef=size(zz_path,1);
Ndata=1e4;
zz_final=zeros(Ndata,7);
count=0;
i1=1:nef;
signv=sign(mean(zz_path(:,5)));%zz_path(kmid(j)+1,5);
ss_path=ssp(i1,:);ss_path=ss_path-ss_path(1);
z0=zz_path(1,:);z0(5:7)=0;
zf=zz_path(end,1:7);zf(5)=0;zf(6)=0;zf(7)=0;
Rtsafe=1; da0=0;
n=100;%min(150,max(25,ceil(abs(ss_path(end))/1)));
m=3;%改成3可行性更好
m0=3;%m0是起始状态优化阶次
obs_ST=[0,0,0];
[DDy_velocity,tt]=planvelocity_son(zz_path,ss_path,z0,zf,ParameterMax,signv,obs_ST,Rtsafe,da0,m,m0,n);
[zzc,~,Nj]=hecheng(DDy_velocity,zz_path,ss_path,tt,Ndata);
if signv<0
    zzc(:,[5,7])=signv*zzc(:,[5,7]);
end
zz_final(max(count+1,1):min(count+Nj,10000),:)=zzc(1:Nj,:);
count=min(count+Nj,1e4);
N_final=min(count,1e4);
end
function [DDy1,tt]=planvelocity_son(zz_path,s_path,z0,~,ParameterMax,signv,obs,Rsafe,da0,m,m0,n)
K=ceil(300/n);
zreal=[0,signv*z0(5),signv*z0(7),da0];
omega_s=zz_path(:,6);
delta_s=zz_path(:,4);
vmax=ParameterMax(1);amax=ParameterMax(2);damax=ParameterMax(3);vamax=min(8,ParameterMax(4));wmax=min(0.32,max(0.1,ParameterMax(5)));
vdmax=ParameterMax(6);%2*0.5;
vwmax=ParameterMax(7);
v0=zreal(2);a0=zreal(3);da0=zreal(4);0;
Smax=max(0.00001,max(s_path)-zreal(1));%real(1));%real(1);
nsk=500;
ss_path=linspace(0,Smax,nsk);
dcurv_path=interp1(s_path,omega_s,ss_path+zreal(1));
delta_path=interp1(s_path,delta_s,ss_path+zreal(1));
vvmax=min(vmax,wmax./(abs(dcurv_path)+0.001));
vvmax=min(vvmax,vdmax./(abs(delta_path)+0.001));
Tmax=sum(Smax/nsk./(vvmax(1:end-1)+vvmax(2:end))*2)+max(vvmax)/amax*2+amax/damax*2+4;
t=(1:n*K)*Tmax/n/K;
ssmax0=ss_path;
v00=v0;
vvmax0=min(vmax,wmax./(abs(dcurv_path)+0.00001));%vvmax0=vvmax01;
vvmax0=min(vvmax0,vdmax./(abs(delta_path)+0.00001));
vvmax0=min(vvmax0,sqrt(vwmax./(abs(dcurv_path)+0.000001)));
if abs(v0*a0)>vamax%强行减小初始速度
    a0=sign(a0)*vamax/v0;
end
Dy00=[0;v0;a0;da0;0];Dy0=Dy00(1:m);
m0=min(m0,m);
[~,~,A0s,b0,A1s,b1,A2s,b2,A3s,b3,D0,D1,D2,D3]=polyfun_dy0(n,Tmax,Dy0,m0,m,K);
A0=[A0s,D0];A1=[A1s,D1];A2=[A2s,D2];A3=[A3s,D3];
a0f=A0(end,:);b0f=b0(end,:);%0<=a0f*u+b0f<=S,
a2f=A2(end,:);b2f=b2(end,:);
lam1=100;0.0001;lam2=0.00001;
lam0=100;100;
lam2f=.001;
lam0f=.001;
H=lam0*(A0'*A0)+lam1*(A1'*A1)+lam2*(A2')*A2+lam2f*(a2f')*a2f+lam0f*(a0f'*a0f);
f=lam0*A0'*(b0-Smax)+lam1*A1'*b1+0*a0f'+lam2*A2'*b2+lam2f*a2f'*b2f+lam0f*(a0f'*(b0f-Smax));%obj=(A2*u+b2)^2-(A0*u+b0)^2;
Dmax=[Smax,vmax,amax,damax,20,20,20,20]';
LB=[-Dmax(m)*ones(n,1);-Dmax(m0+1:m)];UB=-1*LB;
u=zeros(n+m-m0,1);
vvmaxk=ones(n*K,1)*vmax;
dvvmaxk=zeros(n*K,1);
lu=n+m-m0;
scalex=ones(lu,1);
id=(1:K:n*K)';
idva=(1:1:n*K)';
lamcon=1;
y0=zeros(n*K,1);
y1=zeros(n*K,1);
y2=zeros(n*K,1);
y3=zeros(n*K,1);
lamv=120;
maxkss=20;
for kss=1:maxkss
    % ep=0.01;
    lamcon=max(1,lamcon*1);
    vmaxA=diag(dvvmaxk)*A0;
    bvmax=-1*dvvmaxk.*(A0*u);
    v=A1*u+b1;a=A2*u+b2;va=v.*a;
    %Ava=diag(v)*A2+diag(a)*A1;
    Ava=diag(v)*A2;
    bva=va-Ava*u;%Ava*u+bva<vamax
    lamva=.1;if kss>2;lamva=100;end
    scaleb=lamcon*[10*ones(size(b0(id)));
        lamv*ones(size(b1(id)));...
        400*ones(size(b2(id)));
        lamva*ones(numel(idva),1);
        .1
        ];
    Rdis=Rsafe;Rt=0.5;
    [ssmin,ssmax,~]=getyobsexpand(obs(:,1:2),[Rdis,Rt],obs(:,3),Tmax,n*K,Smax,0);%
    if min(ssmax)<Smax
        1;
    end
    if max(ssmin)>0
        1;
    end
    As=[A0(id,:);
        A1(id,:)-vmaxA(id,:);
        A2(id,:);
        Ava(idva,:);
        a0f
        ];
    epf0=0.1;
    bmin=[ssmin(id)-b0(id,:);
        -0*vvmaxk(id,:)-b1(id,:)+bvmax(id,:);
        -amax-b2(id,:);
        -vamax-bva(idva,:);
        Smax-epf0-b0f
        ];
    bmax=[ssmax(id)-b0(id,:);
        vvmaxk(id,:)-b1(id,:)+bvmax(id,:);
        1*amax-b2(id,:);
        vamax-bva(idva,:);
        Smax+epf0-b0f
        ];
    maxiters=200;
    [uuopt]=myquadprog(u,H,f,As,bmin,bmax,LB,UB,maxiters,scaleb,1000*scalex,1.9,0.75,0.001,1e-5);
    if kss==1;eta=1;else;eta=0.7;end
    u=eta*uuopt+(1-eta)*u;
    y0(1:n*K)=A0*u+b0;
    y1(1:n*K)=A1*u+b1;
    y2(1:n*K)=A2*u+b2;
    y3(1:n*K)=A3*u+b3;
    vvmaxk(1:n*K)=interp1(ssmax0,min(vmax,1*vvmax0),max(0,min(Smax,y0)));
    ep=0.0001;vvmaxkep=interp1(ssmax0,min(vmax,vvmax0),max(0,min(Smax,y0+ep)));dvvmaxk=(vvmaxkep-vvmaxk)/ep;
    vareal=max(abs(y1.*y2));
    if max((y1-vvmaxk))>0.1
        lamv=min(1400,lamv*2);
    end
end
tt=[0;t(:)];
DDy=[0,v0,a0,da0;y0,y1,y2,y3]';
DDy1=DDy(1:m,1:n*K+1);
end
function [A,b,A0,b0,A1,b1,A2,b2,A3,b3]=polyfun(n,Xmax,Dy0,m,K)
[A,Aw]=polyodefun(n,Xmax,m,K);
b=Aw*Dy0;nk=n*K;
i=0;id=i+1:m:m*nk;A0=A(id,:);b0=b(id,:);
i=1;id=i+1:m:m*nk;A1=A(id,:);b1=b(id,:);
b3=b0*0;
if nargout>6
    i=2;id=i+1:m:m*nk;A2=A(id,:);b2=b(id,:);
    if m>3
        i=3;id=i+1:m:m*nk;A3=A(id,:);b3=b(id,:);
    else
        A3=zeros(nk,n);
        for i=1:n
            id=(i-1)*K+1:i*K;A3(id,i)=1;b3=b0*0;
        end
    end
end
end
function [A,b,A0,b0,A1,b1,A2,b2,A3,b3]=polyfunB(n,Xmax,m,K)
[A,Aw]=polyodefun(n,Xmax,m,K);
b=Aw;nk=n*K;
i=0;id=i+1:m:m*nk;A0=A(id,:);b0=b(id,:);
i=1;id=i+1:m:m*nk;A1=A(id,:);b1=b(id,:);
A3=A0*0;b3=b0*0;
A2=A0*0;b2=b0*0;
if nargout>6
    if m>2;i=2;id=i+1:m:m*nk;A2=A(id,:);b2=b(id,:);
    else;A2=zeros(nk,n);for i=1:n;id=(i-1)*K+1:i*K;A2(id,i)=1;b2=b0*0;end
    end
    if m>3
        i=3;id=i+1:m:m*nk;A3=A(id,:);b3=b(id,:);
    elseif m==3
        A3=zeros(nk,n);
        for i=1:n
            id=(i-1)*K+1:i*K;A3(id,i)=1;b3=b0*0;
        end
    end
end
end
function [A,b,A0,b0,A1,b1,A2,b2,A3,b3,D0,D1,D2,D3]=polyfun_dy0(n,Xmax,Dy0,m0,m,K)
[A,Aw]=polyodefun(n,Xmax,m,K);
b=Aw(:,1:m0)*Dy0(1:m0);
D=Aw(:,m0+1:m);
nk=n*K;
i=0;id=i+1:m:m*nk;A0=A(id,:);b0=b(id,:);
if m0>=m;D0=[];else;D0=D(id,:);end
i=1;id=i+1:m:m*nk;A1=A(id,:);b1=b(id,:);if m0>=m;D1=[];else;D1=D(id,:);end
b3=b0*0;
if nargout>6
    i=2;id=i+1:m:m*nk;A2=A(id,:);b2=b(id,:);D2=D(id,:);
    if m>3
        i=3;id=i+1:m:m*nk;A3=A(id,:);b3=b(id,:);if m0>=m;D3=[];else;D3=D(id,:);end
    else
        A3=zeros(nk,n);D3=zeros(nk,m-m0);b3=b0*0;
        for i=1:n
            id=(i-1)*K+1:i*K;A3(id,i)=1;
        end
    end
end
end
function [gxxu,gxx0]=polyodefun(n,Tf,m,K)
lx=m;lu=1;%[n,lu]=size(uu);
gxu=zeros(lx,n*lu);
gxxu=zeros(lx*n*K,n*lu);
gxx0=zeros(lx*n*K,lx);gx0=eye(m);
indexu=reshape(1:n*lu,n,lu);
Ti=Tf/n/K;
[A,B]=getab(Ti,m);
for i=1:n
    for j=1:K
        idu=indexu(i,:);
        gxu(:,idu)=A*gxu(:,idu)+B;
        idu0=indexu(1:i-1,:);idu0=idu0(:);
        gxu(:,idu0)=A*gxu(:,idu0);%状态对控制量的梯度
        gx0=A*gx0;
        i2=((i-1)*K+j-1);
        idx=i2*lx+1:i2*lx+lx;gxxu(idx,:)=gxu;gxx0(idx,:)=gx0;
    end
end
end
function [zl]=ctol(zc)
zf=ctof(zc);
zr=ctor(zc);
zl=1/2*(zf+zr);
zl(:,3)=zc(:,3)-zc(:,4)/2;
end